#include "rads_database.hpp"
std::mutex g_db_mutex;
//接受网络传来指令存库
void pradscmdCallback(const rads::rads_cmd::ConstPtr &msg){
    tcp_cmd cmd;
    cmd.type = 1;
    cmd.cmd = msg->pro_nema;
    cmd.ctime = getCurrentTimeString();
    if (insert_cmd(cmd)) {
      //  ROS_INFO("cmd saved successfully");
    } else {
        ROS_ERROR("Failed to save cmd");
    }    
}
//接受传感器数据（rtk+激光雷达数据）存库
void pradsfusionCallback(const rads::rads_trackext::ConstPtr &msg){
    car_log car_log_data;
    car_log_data.speed = isnan(msg->groud_speed) ? 0.0 : msg->groud_speed;
    car_log_data.x = isnan(msg->x) ? 0.0 : msg->x;
    car_log_data.y = isnan(msg->y) ? 0.0 : msg->y;
    car_log_data.z = isnan(msg->z) ? 0.0 : msg->z;
    car_log_data.lidar_heading = isnan(quaternionToEulerAngle(msg->ox,msg->oy,msg->oz,msg->ow))? 0.0 : quaternionToEulerAngle(msg->ox,msg->oy,msg->oz,msg->ow);
    car_log_data.ndt_matching = isnan(msg->matching_rate) ? 0.0 : msg->matching_rate;
    car_log_data.rtk_x = isnan(msg->rtk_x) ? 0.0 : msg->rtk_x;
    car_log_data.rtk_y = isnan(msg->rtk_y) ? 0.0 : msg->rtk_y;
    car_log_data.rtk_z = isnan(msg->rtk_z) ? 0.0 : msg->rtk_z;
    car_log_data.lng = isnan(msg->longitude) ? 0.0 : msg->longitude;
    car_log_data.lat = isnan(msg->latitude) ? 0.0 : msg->latitude;
    car_log_data.rtk_heading = isnan(msg->heading) ? 0.0 : msg->heading;
    car_log_data.position_flag = isnan(msg->posi_flag) ? 0.0 : msg->posi_flag;
    car_log_data.orientation_flag = isnan(msg->head_flag) ? 0.0 : msg->head_flag;
    car_log_data.ctime = getCurrentTimeString();
    if (insert_car_log(car_log_data)) {
       // ROS_INFO("car_log saved successfully");
    } else {
        ROS_ERROR("Failed to save car_log");
    }  
}
//sradsack话题功能：1.
void sradsackCallback(const rads::rads_acks::ConstPtr &msg){
    
   //设置开机点
   if(msg->cmd==16 && msg->ack==1 && msg->ext2==1 ){
    start_machine start_machine_data;
    start_machine_data.map_id = msg->map_id;
    if (!msg->trajectory.poses.empty()) {
        start_machine_data.x = msg->trajectory.poses[0].pose.position.x;
        start_machine_data.y = msg->trajectory.poses[0].pose.position.y;
        start_machine_data.z = msg->trajectory.poses[0].pose.position.z;
        start_machine_data.ox = msg->trajectory.poses[0].pose.orientation.x;
        start_machine_data.oy = msg->trajectory.poses[0].pose.orientation.y;
        start_machine_data.oz = msg->trajectory.poses[0].pose.orientation.z;
        start_machine_data.ow = msg->trajectory.poses[0].pose.orientation.w;
    }
    if (!msg->trajectory_ext.empty()) {
        start_machine_data.num=msg->trajectory_ext[0].point_count;
        start_machine_data.ndt_matching = msg->trajectory_ext[0].matching_rate;
        start_machine_data.rtk_x=msg->trajectory_ext[0].rtk_x;
        start_machine_data.rtk_y=msg->trajectory_ext[0].rtk_y;
        start_machine_data.rtk_z=msg->trajectory_ext[0].rtk_z;
        start_machine_data.lng = msg->trajectory_ext[0].longitude;
        start_machine_data.lat = msg->trajectory_ext[0].latitude;
        start_machine_data.rtk_heading = msg->trajectory_ext[0].heading;
        start_machine_data.position_flag = msg->trajectory_ext[0].posi_flag;
        start_machine_data.orientation_flag = msg->trajectory_ext[0].head_flag;
        start_machine_data.lidar_heading = msg->trajectory_ext[0].ld_heading;
    }
    if (insert_start_machine(start_machine_data)){
        ROS_INFO("start_machine saved successfully");
    } else {
        ROS_ERROR("Failed to save start_machine");
    }  
       
   }
   //删除开机点
   if(msg->cmd==16 && msg->ack==1 && msg->ext2==0 ){
    start_machine start_machine_data;
    start_machine_data.map_id = msg->map_id;
    start_machine_data.num=msg->ext1;
    if (delete_start_machine(start_machine_data)){
        ROS_INFO("delete_start_machine saved successfully");
    } else {
        ROS_ERROR("Failed to save delete_start_machine");
    } 
   }
   //创建路线
   if(msg->cmd==12 && msg->ack==2 && msg->ext2==1 ){
    car car_data;
    car_data.map_id = msg->map_id;
    car_data.line_id = msg->path_id;
    if (!msg->trajectory_ext.empty()) {
        car_data.x=msg->trajectory_ext[0].x;
        car_data.y = msg->trajectory_ext[0].y;
        car_data.z=msg->trajectory_ext[0].z;
        car_data.lidar_heading=msg->trajectory_ext[0].ld_heading;
        car_data.ndt_matching=msg->trajectory_ext[0].matching_rate;
        car_data.rtk_x=msg->trajectory_ext[0].rtk_x;
        car_data.rtk_y=msg->trajectory_ext[0].rtk_y;
        car_data.rtk_z=msg->trajectory_ext[0].rtk_z;
        car_data.rtk_heading = msg->trajectory_ext[0].heading;
        car_data.lng = msg->trajectory_ext[0].longitude;
        car_data.lat = msg->trajectory_ext[0].latitude;
        car_data.position_flag = msg->trajectory_ext[0].posi_flag;
        car_data.orientation_flag = msg->trajectory_ext[0].head_flag;
        car_data.num_point=msg->trajectory_ext[0].point_count;
        car_data.stop_flag=0;
        car_data.stop_time=0;
    }
    if (insert_car(car_data)){
        ROS_INFO("car saved successfully (num_point:%d)",car_data.num_point);
    } else {
        ROS_ERROR("Failed to save car");
    }  
   }
   //马踏飞燕
   if(msg->cmd==888){
    point_all point_all_data;
    point_all_data.map_id = msg->map_id;
    if (!msg->trajectory_ext.empty()) {
        point_all_data.x=msg->trajectory_ext[0].x;
        point_all_data.y = msg->trajectory_ext[0].y;
        point_all_data.z=msg->trajectory_ext[0].z;
        point_all_data.lidar_heading=msg->trajectory_ext[0].ld_heading;
        point_all_data.ndt_matching=msg->trajectory_ext[0].matching_rate;
        point_all_data.rtk_x=msg->trajectory_ext[0].rtk_x;
        point_all_data.rtk_y=msg->trajectory_ext[0].rtk_y;
        point_all_data.rtk_z=msg->trajectory_ext[0].rtk_z;
        point_all_data.lng = msg->trajectory_ext[0].longitude;
        point_all_data.lat = msg->trajectory_ext[0].latitude;
        point_all_data.rtk_heading = msg->trajectory_ext[0].heading;
        point_all_data.position_flag = msg->trajectory_ext[0].posi_flag;
        point_all_data.orientation_flag = msg->trajectory_ext[0].head_flag;
       
    }
    if (insert_point_all(point_all_data)){
        ROS_INFO("point_all saved successfully");
    } else {
        ROS_ERROR("Failed to save point_all");
    } 
   }
   //goback踩点
   if(msg->cmd==999){
    go_back go_back_data;
    if (!msg->trajectory_ext.empty()) {
        go_back_data.x=msg->trajectory_ext[0].x;
        go_back_data.y = msg->trajectory_ext[0].y;
        go_back_data.z=msg->trajectory_ext[0].z;
        go_back_data.lidar_heading=msg->trajectory_ext[0].ld_heading;
        go_back_data.rtk_x=msg->trajectory_ext[0].rtk_x;
        go_back_data.rtk_y=msg->trajectory_ext[0].rtk_y;
        go_back_data.rtk_z=msg->trajectory_ext[0].rtk_z;
        go_back_data.rtk_heading = msg->trajectory_ext[0].heading;
        go_back_data.lng = msg->trajectory_ext[0].longitude;
        go_back_data.lat = msg->trajectory_ext[0].latitude;
        go_back_data.position_flag = msg->trajectory_ext[0].posi_flag;
        go_back_data.orientation_flag = msg->trajectory_ext[0].head_flag;
    }
    if (insert_go_back(go_back_data)){
        ROS_INFO("go_back saved successfully");
    } else {
        ROS_ERROR("Failed to save go_back");
    } 
   }
   //保存任意点路线点
   if(msg->cmd==30 && msg->ack==2 && msg->ext2!=0 ){
    delete_point_data();
    point_data point_data_data;
    bool all_success = true;
    for (const auto& ext : msg->trajectory_ext) {
        point_data_data.x = ext.x;
        point_data_data.y = ext.y;
        point_data_data.z = ext.z;
        point_data_data.lidar_heading = ext.ld_heading;
        point_data_data.ndt_matching = ext.matching_rate;
        point_data_data.rtk_x = ext.rtk_x;
        point_data_data.rtk_y = ext.rtk_y;
        point_data_data.rtk_z = ext.rtk_z;
        point_data_data.lng = ext.longitude;
        point_data_data.lat = ext.latitude;
        point_data_data.rtk_heading = ext.heading;
        point_data_data.position_flag = ext.posi_flag;
        point_data_data.orientation_flag = ext.head_flag;
        point_data_data.num_point = ext.point_count;
        point_data_data.alltotal = ext.alltotal;
        // 存入数据库
        if (!insert_point_data(point_data_data)) {
            ROS_ERROR("Failed to save point_data point (count: %d)", ext.point_count);
            all_success = false;
        }
    }

    if (all_success) {
        ROS_INFO("All %zu trajectory point_data saved successfully", msg->trajectory_ext.size());
    } else {
        ROS_WARN("Some trajectory point_data failed to save");
    }  
       
   }
   //结束建图，保存地图编号到数据库
   if(msg->cmd==10 && msg->ack==1 && msg->ext2==0 ){
        fist_pose fist_pose_data;
        fist_pose_data.map_id=msg->map_id;
        // 存入数据库
        if (insert_fist_pose(fist_pose_data)){
        ROS_INFO("fist_pose saved successfully");
        } else {
            ROS_ERROR("Failed to save fist_pose");
        }  
   }

}
//接受自动驾驶控车信息
void radsdrvingCallback(const rads::rads_driving::ConstPtr &msg){
    car_driving_log car_driving_log_data;
    car_driving_log_data.model = isnan(msg->model)? 0.0 : msg->model;
    car_driving_log_data.steering = isnan(msg->steering)? 0.0 : msg->steering;
    car_driving_log_data.acc = isnan(msg->acc)? 0.0 : msg->acc;
    car_driving_log_data.brake = isnan(msg->brake)? 0.0 : msg->brake;
    car_driving_log_data.gear = isnan(msg->gear)? 0.0 : msg->gear;
    car_driving_log_data.target_speed = isnan(msg->target_speed)? 0.0 : msg->target_speed;
    car_driving_log_data.rpml = isnan(msg->rpml)? 0.0 : msg->rpml;
    car_driving_log_data.rpmr = isnan(msg->rpmr)? 0.0 : msg->rpmr;
    car_driving_log_data.lignt = isnan(msg->lignt)? 0.0 : msg->lignt;
    car_driving_log_data.audio = isnan(msg->audio)? 0.0 : msg->audio;
    car_driving_log_data.sign = isnan(msg->sign)? 0.0 : msg->sign;;
    car_driving_log_data.camera_hor = isnan(msg->camera_hor)? 0.0 : msg->camera_hor;
    car_driving_log_data.camera_ver = isnan(msg->camera_ver)? 0.0 : msg->camera_ver;
    car_driving_log_data.unknow_1 = isnan(msg->unknow_1)? 0.0 : msg->unknow_1;
    car_driving_log_data.unknow_2 = isnan(msg->unknow_2)? 0.0 : msg->unknow_2;
    car_driving_log_data.unknow_3 = isnan(msg->unknow_3)? 0.0 : msg->unknow_3;
    car_driving_log_data.unknow_4 = isnan(msg->unknow_4)? 0.0 : msg->unknow_4;
    car_driving_log_data.unknow_5 = isnan(msg->unknow_5)? 0.0 : msg->unknow_5;
    car_driving_log_data.map_id = isnan(msg->map_id)? 0.0 : msg->map_id;
    car_driving_log_data.line_id = isnan(msg->line_id)? 0.0 : msg->line_id;
    car_driving_log_data.num_point = isnan(msg->num_point)? 0.0 : msg->num_point;
    car_driving_log_data.alltotal = isnan(msg->alltotal)? 0.0 : msg->alltotal;
    car_driving_log_data.ctime = getCurrentTimeString();
    if (insert_driving_log(car_driving_log_data)) {
       // ROS_INFO("driving_log saved successfully");
    } else {
        ROS_ERROR("Failed to save driving_log");
    } 
}

// 下位机来的数据
void pradsscmdCallback(const rads::rads_scmd::ConstPtr &msg)
{
    pcb_log pcb_log_data;
    pcb_log_data.model = isnan(msg->model)? 0.0 : msg->model;
    pcb_log_data.soc = isnan(msg->soc)? 0.0 : msg->soc; 
    pcb_log_data.err = isnan(msg->err)? 0.0 : msg->err; 
    pcb_log_data.rpm_l = isnan(msg->rpm_l)? 0.0 : msg->rpm_l; 
    pcb_log_data.rpm_r = isnan(msg->rpm_r)? 0.0 : msg->rpm_r; 
    pcb_log_data.key_1 = isnan(msg->key_1)? 0.0 : msg->key_1; 
    pcb_log_data.key_2 = isnan(msg->key_2)? 0.0 : msg->key_2; 
    pcb_log_data.key_3 = isnan(msg->key_3)? 0.0 : msg->key_3; 
    pcb_log_data.key_4 = isnan(msg->key_4)? 0.0 : msg->key_4; 
    pcb_log_data.key_5 = isnan(msg->key_5)? 0.0 : msg->key_5; 
    pcb_log_data.key_6 = isnan(msg->key_6)? 0.0 : msg->key_6; 
    pcb_log_data.key_7 = isnan(msg->key_7)? 0.0 : msg->key_7; 
    pcb_log_data.key_8 = isnan(msg->key_8)? 0.0 : msg->key_8; 
    pcb_log_data.ladar_1 = isnan(msg->ladar_1)? 0.0 : msg->ladar_1; 
    pcb_log_data.ladar_2 = isnan(msg->ladar_2)? 0.0 : msg->ladar_2; 
    pcb_log_data.ladar_3 = isnan(msg->ladar_3)? 0.0 : msg->ladar_3; 
    pcb_log_data.ladar_4 = isnan(msg->ladar_4)? 0.0 : msg->ladar_4; 
    pcb_log_data.ladar_5 = isnan(msg->ladar_5)? 0.0 : msg->ladar_5; 
    pcb_log_data.ladar_6 = isnan(msg->ladar_6)? 0.0 : msg->ladar_6; 
    pcb_log_data.ladar_7 = isnan(msg->ladar_7)? 0.0 : msg->ladar_7; 
    pcb_log_data.ladar_8 = isnan(msg->ladar_8)? 0.0 : msg->ladar_8; 
    pcb_log_data.unknow_1 = isnan(msg->unknow_1)? 0.0 : msg->unknow_1; 
    pcb_log_data.unknow_2 = isnan(msg->unknow_2)? 0.0 : msg->unknow_2; 
    pcb_log_data.unknow_3 = isnan(msg->unknow_3)? 0.0 : msg->unknow_3; 
    pcb_log_data.unknow_4 = isnan(msg->unknow_4)? 0.0 : msg->unknow_4; 
    pcb_log_data.unknow_5 = isnan(msg->unknow_5)? 0.0 : msg->unknow_5; 
    pcb_log_data.unknow_6 = isnan(msg->unknow_6)? 0.0 : msg->unknow_6; 
    pcb_log_data.heartbeat = isnan(msg->heartbeat)? 0.0 : msg->heartbeat; 
    pcb_log_data.ctime = getCurrentTimeString();
    if (insert_pcb_log(pcb_log_data)) {
       // ROS_INFO("pcb_log saved successfully");
    } else {
        ROS_ERROR("Failed to save pcb_log");
    }  

}
int main(int argc, char** argv)
{
    ros::init(argc, argv, NODE_DATABASE);
    ros::NodeHandle nh_;

    //订阅网络发来指令(存库)
    static ros::Subscriber pradscmd = nh_.subscribe("pradscmd", 1000, pradscmdCallback);
    //订阅融合后的雷达+RTK数据（存库车辆实时的状态）
    static ros::Subscriber pradsfusion = nh_.subscribe("pradsfusion", 1, pradsfusionCallback); 
    //订阅踩点信息（主要用于响应网络指令，上传地图，上传路线点等）
    static ros::Subscriber sradsack = nh_.subscribe("sradsack", 1, sradsackCallback); 
    // 订阅控制车辆的信息（存储控制车的信息）
    static ros::Subscriber radsdrving = nh_.subscribe("radsdrving", 1, radsdrvingCallback);
    // 订阅下位机发来的指令 pradsscmd
    static ros::Subscriber pradsscmd = nh_.subscribe("pradsscmd", 1, pradsscmdCallback); 
    ros::spin();
    return 0;
}